﻿using System.Collections.Generic;
using System.Linq;

namespace Raven
{
    public class RavenPathPlanner : IPathPlanner
    {
        private const int no_closest_node_found = -1;

        private RavenBot _owner;

        private SparseGraph<NavGraphNode<Trigger<RavenBot>>, NavGraphEdge> _navGraph;

        //a pointer to an instance of the current graph search algorithm.
        private GraphSearchTimeSliced<NavGraphEdge> _currentSearch;

        private Vector2 _destinationPos;

        public RavenPathPlanner(RavenBot bot)
        {
            _owner = bot;
            _navGraph = _owner.GetWorld().GetMap().GetNavGraph();
        }

        public PathSearch CycleOnce()
        {
            Utility.Assert(_currentSearch != null, "<Raven_PathPlanner::CycleOnce>: No search object instantiated");

            PathSearch result = _currentSearch.CycleOnce();

            //let the bot know of the failure to find a path
            if (result == PathSearch.target_not_found)
            {
                _owner.GetWorld().Dispatcher.DispatchMsg(Constants.SEND_MSG_IMMEDIATELY,
                    Constants.SENDER_ID_IRRELEVANT,
                    _owner.ID(),
                    MessageType.Msg_NoPathAvailable);
            }

            //let the bot know a path has been found
            else if (result == PathSearch.target_found)
            {
                //if the search was for an item type then the final node in the path will
                //represent a giver trigger. Consequently, it's worth passing the pointer
                //to the trigger in the extra info field of the message. (The pointer
                //will just be NULL if no trigger)
                Trigger<RavenBot> pTrigger =
                    _navGraph.GetNode(_currentSearch.GetPathToTarget().Last.Value).ExtraInfo();

                _owner.GetWorld().Dispatcher.DispatchMsg(Constants.SEND_MSG_IMMEDIATELY,
                    Constants.SENDER_ID_IRRELEVANT,
                    _owner.ID(),
                    MessageType.Msg_PathReady,
                    pTrigger);
            }

            return result;
        }

        //creates an instance of the Dijkstra's time-sliced search and registers 
        //it with the path manager
        public bool RequestPathToPosition(Vector2 TargetPos)
        {
            Logger.Log("------------------------------------------------");
            GetReadyForNewSearch();

            //make a note of the target position.
            _destinationPos = TargetPos;

            //if the target is walkable from the bot's position a path does not need to
            //be calculated, the bot can go straight to the position by ARRIVING at
            //the current waypoint
            if (_owner.CanWalkTo(TargetPos))
            {
                return true;
            }

            //find the closest visible node to the bots position
            int ClosestNodeToBot = GetClosestNodeToPosition(_owner.Pos());

            //remove the destination node from the list and return false if no visible
            //node found. This will occur if the navgraph is badly designed or if the bot
            //has managed to get itself *inside* the geometry (surrounded by walls),
            //or an obstacle.
            if (ClosestNodeToBot == no_closest_node_found)
            {
                Logger.Log("No closest node to bot found!");

                return false;
            }

            Logger.Log("Closest node to bot is " + ClosestNodeToBot);

            //find the closest visible node to the target position
            int ClosestNodeToTarget = GetClosestNodeToPosition(TargetPos);

            //return false if there is a problem locating a visible node from the target.
            //This sort of thing occurs much more frequently than the above. For
            //example, if the user clicks inside an area bounded by walls or inside an
            //object.
            if (ClosestNodeToTarget == no_closest_node_found)
            {
                Logger.Log("No closest node to target (" + ClosestNodeToTarget + ") found!");

                return false;
            }

            Logger.Log("Closest node to target is " + ClosestNodeToTarget);

            //create an instance of a the distributed A* search class
            _currentSearch = new GraphSearchAStarTS(_navGraph,
                ClosestNodeToBot,
                ClosestNodeToTarget);

            //and register the search with the path manager
            _owner.GetWorld().GetPathManager().Register(this);

            return true;
        }

        //------------------------------ GetReadyForNewSearch -----------------------------------
        //
        //  called by the search manager when a search has been terminated to free
        //  up the memory used when an instance of the search was created
        //-----------------------------------------------------------------------------
        private void GetReadyForNewSearch()
        {
            //unregister any existing search with the path manager
            _owner.GetWorld().GetPathManager().UnRegister(this);

            //clean up memory used by any existing search
            _currentSearch = null;
        }

        //------------------------ GetClosestNodeToPosition ---------------------------
        //
        //  returns the index of the closest visible graph node to the given position
        //-----------------------------------------------------------------------------
        private int GetClosestNodeToPosition(Vector2 pos)
        {
            Fix ClosestSoFar = Fix.MaxValue;
            int ClosestNode = no_closest_node_found;

            //when the cell space is queried this the the range searched for neighboring
            //graph nodes. This value is inversely proportional to the density of a 
            //navigation graph (less dense = bigger values)
            Fix range = _owner.GetWorld().GetMap().GetCellSpaceNeighborhoodRange();

            //calculate the graph nodes that are neighboring this position
            _owner.GetWorld().GetMap().GetCellSpace().CalculateNeighbors(pos, range);

            //iterate through the neighbors and sum up all the position vectors
            foreach (var pN in _owner.GetWorld().GetMap().GetCellSpace())
            {
                //if the path between this node and pos is unobstructed calculate the
                //distance
                if (_owner.CanWalkBetween(pos, pN.Pos()))
                {
                    Fix dist = (pos - pN.Pos()).sqrMagnitude;

                    //keep a record of the closest so far
                    if (dist < ClosestSoFar)
                    {
                        ClosestSoFar = dist;
                        ClosestNode = pN.Index();
                    }
                }
            }

            return ClosestNode;
        }

        public LinkedList<PathEdge> GetPath()
        {
            Utility.Assert(_currentSearch != null, "<Raven_PathPlanner::GetPathAsNodes>: no current search");

            LinkedList<PathEdge> path = _currentSearch.GetPathAsPathEdges();

            int closest = GetClosestNodeToPosition(_owner.Pos());

            path.AddFirst(new PathEdge(_owner.Pos(),
                GetNodePosition(closest),
                0, 0));


            //if the bot requested a path to a location then an edge leading to the
            //destination must be added
            if (_currentSearch.GetSearchType() == SearchType.AStar)
            {
                path.AddLast(new PathEdge(path.Last().Destination(),
                    _destinationPos,
                    0, 0));
            }

            //smooth paths if required
            if (Constants.SmoothPathsQuick)
            {
                SmoothPathEdgesQuick(path);
            }

            if (Constants.SmoothPathsPrecise)
            {
                SmoothPathEdgesPrecise(path);
            }

            return path;
        }

        //------------------------------ RequestPathToItem -----------------------------
        //
        // Given an item type, this method determines the closest reachable graph node
        // to the bot's position and then creates a instance of the time-sliced 
        // Dijkstra's algorithm, which it registers with the search manager
        //
        //-----------------------------------------------------------------------------
        public bool RequestPathToItem(EntityType entityType)
        {
            //clear the waypoint list and delete any active search
            GetReadyForNewSearch();

            //find the closest visible node to the bots position
            int ClosestNodeToBot = GetClosestNodeToPosition(_owner.Pos());

            //remove the destination node from the list and return false if no visible
            //node found. This will occur if the navgraph is badly designed or if the bot
            //has managed to get itself *inside* the geometry (surrounded by walls),
            //or an obstacle
            if (ClosestNodeToBot == no_closest_node_found)
            {
                Logger.Log("No closest node to bot found!");
                return false;
            }

            _currentSearch = new GraphSearchDijkstrasTS(_navGraph,
                ClosestNodeToBot,
                idx =>
                {
                    bool bSatisfied = false;

                    //get a reference to the node at the given node index
                    var node = _navGraph.GetNode(idx);

                    //if the extrainfo field is pointing to a giver-trigger, test to make sure 
                    //it is active and that it is of the correct type.
                    if (node.ExtraInfo() != null &&
                        node.ExtraInfo().isActive() &&
                        node.ExtraInfo().GetEntityType() == entityType)
                    {
                        bSatisfied = true;
                    }

                    return bSatisfied;
                });

            //register the search with the path manager
            _owner.GetWorld().GetPathManager().Register(this);

            return true;
        }

        //------------------------------ GetNodePosition ------------------------------
        //
        //  used to retrieve the position of a graph node from its index. (takes
        //  into account the enumerations 'non_graph_source_node' and 
        //  'non_graph_target_node'
        //----------------------------------------------------------------------------- 
        public Vector2 GetNodePosition(int idx)
        {
            return _navGraph.GetNode(idx).Pos();
        }

        //----------------------- SmoothPathEdgesPrecise ---------------------------------
        //
        //  smooths a path by removing extraneous edges.
        //-----------------------------------------------------------------------------
        public void SmoothPathEdgesPrecise(LinkedList<PathEdge> path)
        {
            //create a couple of iterators
            LinkedListNode<PathEdge> e1, e2;

            //point e1 to the beginning of the path
            e1 = path.First;

            while (e1 != null)
            {
                //point e2 to the edge immediately following e1
                e2 = e1;
                e2 = e2.Next;

                //while e2 is not the last edge in the path, step through the edges
                //checking to see if the agent can move without obstruction from the 
                //source node of e1 to the destination node of e2. If the agent can move
                //between those positions then the any edges between e1 and e2 are
                //replaced with a single edge.
                while (e2 != null)
                {
                    //check for obstruction, adjust and remove the edges accordingly
                    if (_owner.CanWalkBetween(e1.Value.Source(), e2.Value.Destination()))
                    {
                        e1.Value.SetDestination(e2.Value.Destination());
                        path.Remove(e2);
                    }

                    e2 = e2.Next;
                }

                e1 = e1.Next;
            }
        }

        //--------------------------- SmoothPathEdgesQuick ----------------------------
        //
        //  smooths a path by removing extraneous edges.
        //-----------------------------------------------------------------------------
        public void SmoothPathEdgesQuick(LinkedList<PathEdge> path)
        {
            LinkedListNode<PathEdge> e1 = path.First;
            LinkedListNode<PathEdge> e2 = e1.Next;

            //while e2 is not the last edge in the path, step through the edges checking
            //to see if the agent can move without obstruction from the source node of
            //e1 to the destination node of e2. If the agent can move between those 
            //positions then the two edges are replaced with a single edge.
            while (e2 != null)
            {
                //check for obstruction, adjust and remove the edges accordingly
                if (_owner.CanWalkBetween(e1.Value.Source(), e2.Value.Destination()))
                {
                    e1.Value.SetDestination(e2.Value.Destination());
                    path.Remove(e2);
                    e2 = e2.Next;
                }

                else
                {
                    e1 = e2;
                    e2 = e2.Next;
                }
            }
        }

        //------------------------ GetCostToClosestItem ---------------------------
        //
        //  returns the cost to the closest instance of the giver type. This method
        //  makes use of the pre-calculated lookup table. Returns -1 if no active
        //  trigger found
        //-----------------------------------------------------------------------------
        public Fix GetCostToClosestItem(EntityType GiverType)
        {
            //find the closest visible node to the bots position
            int nd = GetClosestNodeToPosition(_owner.Pos());

            //if no closest node found return failure
            if (nd == Constants.INVALID_NODE_INDEX) return -1;

            Fix ClosestSoFar = Fix.MaxValue;

            //iterate through all the triggers to find the closest *active* trigger of 
            //type GiverType
            foreach (var it in _owner.GetWorld().GetMap().GetTriggers())
            {
                if (it.GetEntityType() == GiverType && it.isActive())
                {
                    Fix cost = _owner.GetWorld().GetMap().CalculateCostToTravelBetweenNodes(nd, it.GraphNodeIndex());

                    if (cost < ClosestSoFar)
                    {
                        ClosestSoFar = cost;
                    }
                }
            }

            //return a negative value if no active trigger of the type found
            if (ClosestSoFar.Equals(Fix.MaxValue))
            {
                return -1;
            }

            return ClosestSoFar;
        }
    }
}